#include "interface.h"
#include "can.h"
#include "referee.h"

interface_speed_cmdTypeDef interface_speed_cmd = {};

uint8_t interface_referee_heat_send()
{
	int16_t surplus_heat = referee_game_robot_status.shooter_barrel_heat_limit - referee_power_heat_data.shooter_17mm_1_barrel_heat;
	
	uint8_t mode = 1;
	uint8_t data[2] = {};
	data[0] = surplus_heat;
	data[1] = surplus_heat >> 8;
	uint32_t CAN_TX_BOX0;
	CAN_TxHeaderTypeDef tx_header;
	tx_header.StdId = 0x151;
	tx_header.IDE = CAN_ID_STD;
	tx_header.RTR = CAN_RTR_DATA;
	tx_header.DLC = 2;
	tx_header.TransmitGlobalTime = DISABLE;
	mode = HAL_CAN_AddTxMessage(&hcan1, &tx_header, data, &CAN_TX_BOX0);
	return mode;
}

void interface_callback(CAN_RxHeaderTypeDef* rx_header, uint8_t* data)
{
	if (rx_header->StdId == 0x111)
	{
		interface_speed_cmd.vx = data[0] | (data[1] << 8);
		interface_speed_cmd.vy = data[2] | (data[3] << 8);
		interface_speed_cmd.vz = data[4] | (data[5] << 8);
		interface_speed_cmd.power_percentage = data[6] | (data[7] << 8);
		HAL_IWDG_Refresh(&hiwdg);
	}
}